#ifndef __ODRIVE_CAN_H
#define __ODRIVE_CAN_H

#include "can.h"
#include "Bsp_CAN.h"
#include "math.h"

//COMMAND ID
#define ODRIVE_HEARTBEAT_MESSAGE		0x001
#define ODRIVE_ESTOP_MESSAGE            0x002
#define SET_AXIS_NODE_ID				0x006
#define SET_AXIS_REQUESTED_STATE 		0x007
#define ENCODER_ESTIMATES				0x009
#define GET_ENCODER_COUNT				0x00A
#define SET_CONTROLLER_MODES			0x00B
#define SET_INPUT_POS					0x00C
#define SET_INPUT_VEL					0x00D
#define SET_INPUT_TORQUE				0x00E
#define SET_LIMITS						0x00F
#define START_ANTICOGGING               0x010
#define GET_IQ							0x014
#define REBOOT_ODRIVE					0x016
#define GET_BUS_VOLTAGE_CURRENT			0x017
#define CLEAR_ERRORS					0x018
#define SET_POSITION_GAIN				0x01A
#define SET_VEL_GAINS					0x01B

//Axis Parameters
typedef struct axis_t
{
    int AXIS_ID;
    float AXIS_Encoder_Pos;
    float AXIS_Encoder_Vel;
    int32_t AXIS_Encoder_CPR;
    int32_t AXIS_Encoder_Shadow;
    float AXIS_Bus_Voltage;
    float AXIS_Bus_Current;
    float AXIS_Iq_Setpoint;
    float AXIS_Iq_Measured;
    uint32_t AXIS_Error;
    uint8_t AXIS_Current_State;
    uint8_t Controller_Status;
} axis_t;

//Axis States
typedef enum {
    UNDEFINED = 0x0,
    IDLE = 0x1,
    STARTUP_SEQUENCE = 0x2,
    FULL_CALIBRATION_SEQUENCE = 0x3,
    MOTOR_CALIBRATION = 0x4,
    ENCODER_INDEX_SEARCH = 0x6,
    ENCODER_OFFSET_CALIBRATION = 0x7,
    CLOSED_LOOP_CONTROL = 0x8,
    LOCKIN_SPIN = 0x9,
    ENCODER_DIR_FIND = 0xA,
    HOMING = 0xB,
    ENCODER_HALL_POLARITY_CALIBRATION = 0xC,
    ENCODER_HALL_PHASE_CALIBRATION = 0xD
} Axis_State;

//Control Modes
typedef enum {
    VOLTAGE_CONTROL = 0x0,
    TORQUE_CONTROL = 0x1,
    VELOCITY_CONTROL = 0x2,
    POSITION_CONTROL = 0x3
} Control_Mode;

//Input Modes
typedef enum {
    INACTIVE = 0x0,
    PASSTHROUGH = 0x1,
    VEL_RAMP = 0x2,
    POS_FILTER = 0x3,
    MIX_CHANNELS = 0x4,
    TRAP_TRAJ = 0x5,
    TORQUE_RAMP = 0x6,
    MIRROR = 0x7,
    TUNING = 0x8
} Input_Mode;

extern uint8_t o_can_rx1[8];


void set_position (CAN_HandleTypeDef* _hcan,uint8_t odrive_axis_can_id,float Input_Pos, int Vel_FF, int Torque_FF);
void set_vel (CAN_HandleTypeDef* _hcan,uint8_t odrive_axis_can_id,float Vel,float Tor_FF);
void set_torque (CAN_HandleTypeDef* _hcan,uint8_t odrive_axis_can_id,float torque);
void Set_Limits(CAN_HandleTypeDef* _hcan,uint8_t odrive_axis_can_id, float vel_lim, float curr_lim);
void Set_Position_Gain(CAN_HandleTypeDef* _hcan, uint8_t odrive_axis_can_id, float pos_gain);
void Set_Position_Gain(CAN_HandleTypeDef* _hcan, uint8_t odrive_axis_can_id, float pos_gain);
void Set_Vel_Gains(CAN_HandleTypeDef* _hcan,uint8_t odrive_axis_can_id, float Vel_Gain, float Vel_Int_Gain);
void Set_Axis_Requested_State(CAN_HandleTypeDef* _hcan,uint8_t odrive_axis_can_id, Axis_State state);
void Set_Controller_Modes(CAN_HandleTypeDef* _hcan,uint8_t odrive_axis_can_id, Control_Mode ControlMode, Input_Mode InputMode);
void Set_Axis_Node_ID(CAN_HandleTypeDef* _hcan,uint8_t odrive_axis_can_id, uint32_t node_id);

void Odrive_ESTOP (CAN_HandleTypeDef* _hcan,uint8_t odrive_axis_can_id);
void Clear_Errors(CAN_HandleTypeDef* _hcan,uint8_t odrive_axis_can_id);
void reboot_odrive (CAN_HandleTypeDef* _hcan,uint8_t odrive_axis_can_id);

void o_can_send_message_Data(CAN_HandleTypeDef* _hcan,uint32_t id,uint8_t* data,uint8_t len);
void o_can_send_message_Remoto(CAN_HandleTypeDef* _hcan,uint32_t id,uint8_t* data,uint8_t len);

#endif

